/**
 * @file motion_predictor_example.cpp
 * @brief Example demonstrating how to use the motion_predictor ROS2 header-only package
 * @author EPSILON Team
 */

#include "rclcpp/rclcpp.hpp"
#include "motion_predictor/onlane_fs_predictor.h"

class MotionPredictorExampleNode : public rclcpp::Node
{
public:
    MotionPredictorExampleNode() : Node("motion_predictor_example")
    {
        RCLCPP_INFO(this->get_logger(), "Motion Predictor Example Node Started");
        
        // Example usage of motion prediction capabilities
        // Note: This is a header-only library, so all functionality is template-based
        
        RCLCPP_INFO(this->get_logger(), "Motion prediction headers included successfully");
        RCLCPP_INFO(this->get_logger(), "Ready to use onlane forward simulation-based prediction");
        
        // Create timer for demonstration
        timer_ = this->create_wall_timer(
            std::chrono::seconds(5),
            std::bind(&MotionPredictorExampleNode::timer_callback, this));
    }

private:
    void timer_callback()
    {
        RCLCPP_INFO(this->get_logger(), "Motion predictor ready for trajectory prediction");
        // Here you would typically:
        // 1. Get current vehicle states from surrounding vehicles
        // 2. Use onlane forward simulation to predict future motions
        // 3. Publish prediction results for planning algorithms
        // 4. Update prediction models based on observed behaviors
    }

    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    
    try {
        auto node = std::make_shared<MotionPredictorExampleNode>();
        rclcpp::spin(node);
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("motion_predictor_example"), 
                     "Exception caught: %s", e.what());
        return 1;
    }
    
    rclcpp::shutdown();
    return 0;
}